# -*- coding: utf-8 -*-

# brief: read .csv exported by JSope and compute PWM duty cycles.
# author: luwei
# date:   2023/03/16

import codecs
import numpy as np
import csv
from collections import namedtuple

def csv_file_data(filename):
    Valpha = []
    Vbeta = []
    duty1 = []
    duty2 = []
    # get filename's suffix: base at [0], suffix at [1]
    suffix = os.path.splitext(filename)[1].lower()
    if suffix == ".csv":
        #fh = open(filename, "rb")
        #efh = codecs.EncodedFile(fh, data_encoding='ascii', file_encoding='utf-8')
        #a = np.genfromtxt(efh, delimiter=';')
        #fh.close()
        #Valpha = a[3:4]
        #Vbeta = a[4:5]
        # Eliminate _csv.Error: field larger than field limit (131072)
        csv.field_size_limit(500*1024*1024)
        with open(filename, encoding='utf-8') as csvfile:
            reader = csv.reader(csvfile, quotechar='#', delimiter=';')
            # Timestamp;Ia;Ib;Valpha;Vbeta;CntPhA;CntPhB;CntPhC;Sector
            headers = next(reader)
            Row = namedtuple('Row', headers)
            for r in reader:
                row = Row(*r)
                #print(row.Valpha, row.Vbeta)
                Valpha.append(row.Valpha)
                Vbeta.append(row.Vbeta)
                duty1.append(row.CntPhA)
                duty2.append(row.CntPhB)
    else:
        print("unsupported file format")
        sys.exit(1)
    return Valpha, Vbeta, duty1, duty2

# ST MCSDK610 pwm_curr_fdbk.c: PWMC_SetPhaseVoltage()
def svpwm_gen_st(alpha, beta):
    PWMperiod = 10500;
    hT_Sqrt3 = (PWMperiod * 0xDDB4) / 16384;
    wUAlpha = alpha * hT_Sqrt3;
    wUBeta = -(beta * PWMperiod) * 2;

    wX = wUBeta;
    wY = (wUBeta + wUAlpha) / 2;
    wZ = (wUBeta - wUAlpha) / 2;
    #print("wX=%d wY=%d wZ=%d" % (wX, wY, wZ));

    # Sector calculation from wX, wY, wZ
    Sector = 0; #unknown
    if (wY < 0):
      if (wZ < 0):
        Sector = 5;#SECTOR_5;
        wTimePhA = (PWMperiod / 4) + ((wY - wZ) / 262144);
        wTimePhB = wTimePhA + (wZ / 131072);
        wTimePhC = wTimePhA - (wY / 131072) ;

        lowDuty = wTimePhC;
        midDuty = wTimePhA;
        highDuty = wTimePhB;
      else: # wZ >= 0
        if (wX <= 0):
          Sector = 4;#SECTOR_4;
          wTimePhA = (PWMperiod / 4) + ((wX - wZ) / 262144);
          wTimePhB = wTimePhA + (wZ / 131072);
          wTimePhC = wTimePhB - (wX / 131072);

          lowDuty = wTimePhC;
          midDuty = wTimePhB;
          highDuty = wTimePhA;
        else: #wX > 0
          Sector = 3;#SECTOR_3;
          wTimePhA = (PWMperiod / 4)+ ((wY - wX) / 262144);
          wTimePhC = wTimePhA - (wY / 131072);
          wTimePhB = wTimePhC + (wX / 131072);

          lowDuty = wTimePhB;
          midDuty = wTimePhC;
          highDuty = wTimePhA;
    else: # wY > 0
      if (wZ >= 0):
        Sector = 2;#SECTOR_2;
        wTimePhA = (PWMperiod / 4) + ((wY - wZ) / 262144);
        wTimePhB = wTimePhA + (wZ / 131072);
        wTimePhC = wTimePhA - (wY / 131072);

        lowDuty = wTimePhB;
        midDuty = wTimePhA;
        highDuty = wTimePhC;
      else: # wZ < 0
        if ( wX <= 0 ):
          Sector = 6;#SECTOR_6;
          wTimePhA = (PWMperiod / 4) + ((wY - wX) / 262144);
          wTimePhC = wTimePhA - (wY / 131072);
          wTimePhB = wTimePhC + (wX / 131072);

          lowDuty = wTimePhA;
          midDuty = wTimePhC;
          highDuty = wTimePhB;
        else: # wX > 0
          Sector = 1;#SECTOR_1;
          wTimePhA = (PWMperiod / 4)+ ((wX - wZ) / 262144);
          wTimePhB = wTimePhA + (wZ / 131072);
          wTimePhC = wTimePhB - (wX / 131072);

          lowDuty = wTimePhA;
          midDuty = wTimePhB;
          highDuty = wTimePhC;

    duty1 = wTimePhA;
    duty2 = wTimePhB;
    duty3 = wTimePhC;
    return duty1, duty2, duty3, Sector;

# SNC snc_foc_q15.c: snc_inv_clarke_swap_q15() snc_svpwm_gen_fast_q15()
def svpwm_gen_snc(alpha, beta):
  combo = 0;
  period = 0x7fff;

  Vr1 = beta;
  beta  = (-beta * 0x4000) / 32768;
  alpha = (alpha * 0x6eda) / 32768;
  Vr2 = beta + alpha;
  Vr3 = beta - alpha;

  # Map from abc sign bits: combo(bit2 bit1 bit0) to SV Sector.
  if (Vr3 >= 0):
      combo = 1;
  if (Vr2 >= 0):
      combo += 2;
  if (Vr1 >= 0):
      combo += 4;

  # Choose Tx,Ty from Vr1,Vr2,Vr3 according Sector no.
  # Duty calculation sequence: Tmin, Tmid, Tmax.
  if (combo == 6): # Sector I
      Sector = 1;
      duty3 = (period - Vr2 - Vr1) / 2;
      duty2 = duty3 + Vr1;
      duty1 = duty2 + Vr2;
  elif combo == 4: # Sector II
      Sector = 2;
      duty3 = (period + Vr2 + Vr3) / 2;
      duty1 = duty3 - Vr3;
      duty2 = duty1 - Vr2;
  elif combo == 5: # Sector III
      Sector = 3;
      duty1 = (period - Vr1 - Vr3) / 2;
      duty3 = duty1 + Vr3;
      duty2 = duty3 + Vr1;
  elif combo == 1: # Sector IV
      Sector = 4;
      duty1 = (period + Vr1 + Vr2) / 2;
      duty2 = duty1 - Vr2;
      duty3 = duty2 - Vr1;
  elif combo == 3: # Sector V
      Sector = 5;
      duty2 = (period - Vr3 - Vr2) / 2;
      duty1 = duty2 + Vr2;
      duty3 = duty1 + Vr3;
  elif combo == 2: # Sector VI
      Sector = 6;
      duty2 = (period + Vr3 + Vr1) / 2;
      duty3 = duty2 - Vr1;
      duty1 = duty3 - Vr3;
  else: # special case for Vr1==Vr2==Vr3==0 due to (Valpha,Vbeta) = (0,0)
      Sector = 0;
      duty1 = duty2 = duty3 = period / 2;
  return duty1, duty2, duty3, Sector;

if __name__ == "__main__":
    import os,sys
    if not (len(sys.argv)>1 and os.path.isfile(sys.argv[1])):
        print('usage: {} jscope_exported_data.csv'.format(sys.argv[0]))
        sys.exit(1)

    # ebf_mcsdk610_IaIb_Valphabeta_CntPhABC_Sector.csv
    print("invClarke, SVPWM generation: ", end='');
    Valpha, Vbeta, dutyA, dutyB = csv_file_data(sys.argv[1])
    num_records = len(Valpha);
    for i in range(0, num_records):
        if num_records < 100:
            print("(Valpha=%d, Vbeta=%d)=>" % (int(Valpha[i]), int(Vbeta[i])));
        csv_ratio1 = int(dutyA[i])/5250.0
        csv_ratio2 = int(dutyB[i])/5250.0
        duty1, duty2, duty3, Sector_st = svpwm_gen_st(int(Valpha[i]), int(Vbeta[i]));
        st_ratio1 = duty1/5250.0;
        st_ratio2 = duty2/5250;
        st_ratio3 = duty3/5250;
        if num_records < 100:
            print("  ST SVPWM Sector%d: %d %d %d\t%f %f %f" % (Sector_st, duty1, duty2, duty3, st_ratio1, st_ratio2, st_ratio3));

        duty1, duty2, duty3, Sector_snc = svpwm_gen_snc(int(Valpha[i]), int(Vbeta[i]));
        snc_ratio1 = duty1/32768.0;
        snc_ratio2 = duty2/32768;
        snc_ratio3 = duty3/32768;
        if num_records < 100:
            print(" SNC SVPWM Sector%d: %d %d %d\t%f %f %f" % (Sector_snc, duty1, duty2, duty3, snc_ratio1, snc_ratio2, snc_ratio3));

        if Sector_st + Sector_snc != 7:
            print("FAIL: Sector %d %d" % (Sector_st, Sector_snc));
            sys.exit(1)
        if abs(st_ratio1 - snc_ratio1) > 0.0001 or abs(st_ratio2 - snc_ratio3) > 0.0001 or abs(st_ratio3 - snc_ratio2) > 0.0001:
            print("FAIL at %d(alpha=%d,beta=%d): ratio=%f,%f,%f;%f,%f,%f" % (i, int(Valpha[i]), int(Vbeta[i]), st_ratio1,st_ratio2,st_ratio3, snc_ratio1,snc_ratio2,snc_ratio3));
            sys.exit(1)
        if abs(csv_ratio1 - st_ratio1) > 0.04 or abs(csv_ratio1 - st_ratio1) > 0.04:
            warns += 1
            if (warns > 2):
                print("WARNING at %d(Sector%d) due to async read: csv=%f,%f; snc=%f,%f" % (i, Sector_st, csv_ratio1,csv_ratio2, snc_ratio1,snc_ratio2))
        else:
            warns = 0

    print("PASS");
